#include "gSensor.h"
#include "math.h"
#include "esp_log.h"
#include "driver/i2c.h"
#include "sdkconfig.h"
#include "commonapi.h"

static float resolution = 3.9;

static void Delay_ms(uint32_t ms) {
	vTaskDelay(ms);
}

static void gSensor_SetResolution()

uint8_t gSensor_WriteRegister(uint8_t reg, uint8_t data)
{
    uint8_t ret = 0;
	i2c_cmd_handle_t cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, GSENSOR_DEVICE, true);
	i2c_master_write_byte(cmd, reg, true);
	i2c_master_write_byte(cmd, data, true);
	i2c_master_stop(cmd);
	esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, 1000 / portTICK_RATE_MS);
	if (ret != ESP_OK) {
		printf("[ERR] %s: %d\n", __func__, __LINE__);
        ret = 1;
	}
	i2c_cmd_link_delete(cmd);
    return ret;
}

uint8_t gSensor_ReadRegister(uint8_t reg, uint8_t* data)
{
    uint8_t ret = 0;
	i2c_cmd_handle_t cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, GSENSOR_DEVICE, true);
	i2c_master_write_byte(cmd, reg, true);
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, GSENSOR_DEVICE + 1, true);
	i2c_master_read_byte(cmd, data, I2C_MASTER_NACK);
	i2c_master_stop(cmd);
	esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, 1000 / portTICK_RATE_MS);
	if (ret != ESP_OK) {
		printf("[ERR] %s: %d\n", __func__, __LINE__);
        ret = 1;
	}
	i2c_cmd_link_delete(cmd);
    return ret;
}

uint8_t gSensor_ReadData(uint8_t  reg, size_t len, uint8_t* data)
{
    uint8_t ret = 0;
	i2c_cmd_handle_t cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, GSENSOR_DEVICE, true);
	i2c_master_write_byte(cmd, reg, true);
	i2c_master_start(cmd);

	i2c_master_write_byte(cmd, GSENSOR_DEVICE + 1, true);

	if (len > 1)
	{
		i2c_master_read(cmd, data, len - 1, I2C_MASTER_ACK);
	}
	i2c_master_read_byte(cmd, &data[len - 1], I2C_MASTER_NACK);

	i2c_master_stop(cmd);
	esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, 1000 / portTICK_RATE_MS);
	if (ret != ESP_OK) {
		printf("[ERR] %s: %d: ret: %d\n", __func__, __LINE__, ret);
        ret = 1;
	}
	i2c_cmd_link_delete(cmd);
    ret = 0;
}

uint8_t gSensor_Init(void)
{
	uint8_t  c = 0;
    uint8_t ret;
	ret = gSensor_ReadRegister(GSENSOR_CHIP_ID, &c);
	if (c == 0x02)
    {
		printf("BMA250 ready\n");
	}
	else
    {
		printf("Could not connect to BMA250\n");
    }
    ret = gSensor_WriteRegister(GSENSOR_INT_CFG_0, 0);
    if(ret != 0)
    {
        printf("GSENSOR_INT_CFG_0 error\n");
    }
    ret += gSensor_WriteRegister(GSENSOR_INT_CFG_1, 0);
    if(ret != 0)
    {
        printf("GSENSOR_INT_CFG_1 error\n");
    }
    ret += gSensor_WriteRegister(GSENSOR_INT_MAP_1, 0x02);
    if(ret != 0)
    {
        printf("GSENSOR_INT_MAP_1 error\n");
    }
    printf("BMA250 init finish\n");
    return ret;
}

uint8_t gSensor_Readxyz(gSensor_data* sample)
{
    unsigned char read_buff[6] = { 0 };

    if(gSensor_ReadData(GSENSOR_DATA_X_L, 6, writebuff) != 0)
    {
        printf("[ERR] %s: %d: ret: %d\n", __func__, __LINE__, ret);
        return 1;
    }

	sample->AcceX = (((int16_t)read_buff[1]) << 8) | read_buff[0];
	sample->AcceY = (((int16_t)read_buff[3]) << 8) | read_buff[2];
	sample->AcceZ = (((int16_t)read_buff[5]) << 8) | read_buff[4];

	sample->AcceX = sample->AcceX >> 6;
	sample->AcceY = sample->AcceY >> 6;
	sample->AcceZ = sample->AcceZ >> 6;
    return 0;
}

uint8_t gSensor_enable_interrupt(uint16_t flag)
{
    uint8_t flag_h = flag >> 8;
    uint8_t flag_l = flag;
    uint8_t ret;

    return ret;
}

float gSensor_get_resolution(void)
{
    return resolution;
}

